
#ifndef COMMON_H_
#define COMMON_H_

#include "interface/enum.h"
#include <iostream>
#include <vector>

// 跟随过程中的状态
BETTER_ENUM(FollowState, uint8_t,
            kInitializing,  // 刚初始化控制器的状态
            kIdle,          // 空闲状态，没有任何任务
            kFollowing,     // 跟随过程中的状态
            kPaused,        // 被任务中止的状态
            kReachGoal,     // 跟随完成的状态
            kFailed         // 路径错误的状态
)

// 定义点结构体
// 表示路径时，yaw和curvature有效
struct Point {
    float x;
    float y;
    float yaw;        // 方向角
    float curvature;  // 曲率
    Point(float _x = 0, float _y = 0) : x(_x), y(_y) {}

    bool  operator==(const Point& other) const { return x == other.x && y == other.y; }
    Point operator+(const Point& oher) const { return {x + oher.x, y + oher.y}; }
    Point operator-(const Point& other) const { return Point(x - other.x, y - other.y); }
    Point operator*(float s) const { return {x * s, y * s}; }
};

using Path = std::vector<Point>;

// 重载<<运算符，使其能够输出Point对象
inline std::ostream& operator<<(std::ostream& os, const Point& p) {
    os << "(" << p.x << ", " << p.y << ")";
    return os;
}

struct Odometry {
    float x;
    float y;
    float yaw;
    float v;  // 线速度
    float w;  // 角速度
};

#endif